#include "Accelerometer.h";
#include "Kalman.h";

// Gyroscope pins.
const int gyroXPin = 1;
const int gyroYPin = 0;
const int gyroZPin = 2;

// Accelerometer pins.
const int accXPin = 5;
const int accYPin = 4;
const int accZPin = 3;

// Timer.
const int pollingPeriod = 10;
int deltaTime = 0;
int previousTime = 0;

// Gyroscope zero rate voltage.
float gyroXZeroRate = 0;
float gyroYZeroRate = 0;
float gyroZZeroRate = 0;

// Accelerometer zero g bias.
float accXZeroBias = 0;
float accYZeroBias = 0;
float accZZeroBias = 0;

float gyroZAngle = 0;

// Kalman angles.
int kalmanAngleX = 0;
int kalmanAngleY = 0;

// Debug mode.
const int debugMode = 1;

FilterData xFilterData;
FilterData yFilterData;

StateData xStateData;
StateData yStateData;

StateData zStateData; // for gyro angle rotation about z axis

AccData xAccData;
AccData yAccData;

long tempMs;

void setup() {
  // Initialization
  analogReference(EXTERNAL);
  Serial.begin(115200);

  // Calibrate sensors.
  if (debugMode == 1) {
    Serial.println("DEBUG: Beginning sensor calibration.");
  }

  delay (100);
  gyroXZeroRate = calibrateSensor(gyroXPin);
  gyroYZeroRate = calibrateSensor(gyroYPin);
  gyroZZeroRate = calibrateSensor(gyroZPin);
  accXZeroBias = calibrateSensor(accXPin);
  accYZeroBias = calibrateSensor(accYPin);
  accZZeroBias = calibrateSensor(accZPin);  

  if (debugMode == 1) {
    Serial.println("DEBUG: Ending sensor calibration.");
    printCalibrationResults();
    printTimerHeader(); 
  }

  // Setup filter.
  xFilterData.p00 = 0;
  xFilterData.p01 = 0;
  xFilterData.p10 = 0;
  xFilterData.p11 = 0;
  xFilterData.x0 = 90.00;
  xFilterData.x1 = 0;   
  yFilterData.p00 = 0;
  yFilterData.p01 = 0;
  yFilterData.p10 = 0;
  yFilterData.p11 = 0;
  yFilterData.x0 = 90.00;
  yFilterData.x1 = 0;    

  // Setup accelerometer
  xAccData.count = 0;
  xAccData.acceleration[0] = 0;
  xAccData.acceleration[1] = 0;
  xAccData.acceleration[2] = 0;
  xAccData.velocity[0] = 0;
  xAccData.velocity[1] = 0;
  xAccData.position[0] = 0;
  xAccData.position[1] = 0; 
  xAccData.positive = -1;  
  yAccData.count = 0;
  yAccData.acceleration[0] = 0;
  yAccData.acceleration[1] = 0;
  yAccData.velocity[0] = 0;
  yAccData.velocity[1] = 0;
  yAccData.position[0] = 0;
  yAccData.position[1] = 0;     
  yAccData.positive = -1;  

  xStateData.q = 0.125;
  xStateData.r = 4;
  xStateData.p = 0;
  xStateData.x = 0;

  yStateData.q = 4;
  yStateData.r = 32;
  yStateData.p = 0;
  yStateData.x = 0;

  zStateData.q = 0.148228026; // covar(X, X) from 1000 data points
  zStateData.r = 4; // just picked a random number..
  zStateData.p = 0;
  zStateData.x = 0;

  // Start timer.
  previousTime = millis();
}

int gyroXRaw;
float gyroXRate;
int gyroYRaw;
float gyroYRate;
int gyroZRaw;
float gyroZRate;
float gyroZRateWithKalman;

int accXRaw;
float accXValue;
int accYRaw;
float accYValue;
int accZRaw;
float accZValue;
float magnitude;
float accXAngle;
float accYAngle;

float correctAccXValue;
float correctAccYValue;

float kalmanAccX;
float kalmanAccY;

float velocityX;
float velocityY;
float velocityZ;

void loop() {

  // Read and convert gyroscope.
  gyroXRaw = readAverageAnalog(gyroXPin, 5, false);
  gyroXRate = rawToRate(gyroXRaw, gyroXZeroRate);

  gyroYRaw = readAverageAnalog(gyroYPin, 5, false);
  gyroYRate = rawToRate(gyroYRaw, gyroYZeroRate);

  gyroZRaw = readAverageAnalog(gyroZPin, 5, false);
  gyroZRate = rawToRate(gyroZRaw, gyroZZeroRate);
  // Z deadzone
  if (gyroZRate < 25.0f && gyroZRate > -25.0f) {
    gyroZRate = 0.0f;
  }
  gyroZRateWithKalman = getAccKalman(&zStateData, gyroZRate);

  // Read and convert accelerometer.  
  accXRaw = readAverageAnalog(accXPin, 5, false);
  accXValue = rawToGravity(accXRaw, accXZeroBias);

  accYRaw = readAverageAnalog(accYPin, 5, false);
  accYValue = rawToGravity(accYRaw, accYZeroBias);

  accZRaw = readAverageAnalog(accZPin, 5, false);
  accZValue = rawToGravity(accZRaw, accYZeroBias);

  magnitude = getMagnitude(accXValue, accYValue, accZValue);

  accXAngle = radianToDegree(acos(accXValue / magnitude));
  accYAngle = radianToDegree(acos(accYValue / magnitude));

  getTrueAngles(&accXAngle, &accYAngle, &accXValue, &accYValue,
    &accZValue);

  kalmanAngleX = getKalman(deltaTime, gyroXRate, accXAngle,
    &xFilterData);
  kalmanAngleY = getKalman(deltaTime, gyroYRate, accYAngle,
    &yFilterData);

  // Cancel gravity before calculating velocity
  getTrueAcc(&correctAccXValue, &correctAccYValue, accXValue,
    accYValue, kalmanAngleX, kalmanAngleY);
    
  float test = sqrt(pow(accZValue, 2) + pow(cos(kalmanAngleY / 57.295779513082320876798154814105), 2) +
    pow(cos(kalmanAngleX / 57.295779513082320876798154814105), 2));;

  kalmanAccX = getAccKalman(&xStateData, correctAccXValue);
  kalmanAccY = getAccKalman(&yStateData, correctAccYValue);
 
  velocityX = getVelocity(deltaTime, kalmanAccX, &xAccData);
  velocityY = getVelocity(deltaTime, kalmanAccY, &yAccData);
  velocityZ = 0;

  printAll(deltaTime, kalmanAngleX, kalmanAngleY, gyroZRateWithKalman,
    -velocityY, velocityX, test, 0, 0);  

  delay(pollingPeriod);

  // Recalculate time.
  tempMs = millis();
  deltaTime = tempMs - previousTime;
  previousTime = tempMs;
}



